/*
实现从乌龟的位姿话题下获取数据，将乌龟位置发布出去，作为子坐标。
关键步骤：
1.订阅乌龟位姿话题
2.编写回调函数，处理乌龟位姿数据，将其转换为坐标变换的消息。
3.创建坐标变换的发布方。
4.组织数据并发布。
*/
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPoseCallback(const turtlesim::Pose::ConstPtr &pose)
{
    //创建坐标变换发布者
    static tf2_ros::TransformBroadcaster dynamic_pub;
    //组织数据
    geometry_msgs::TransformStamped tfs_data;
    tfs_data.header.frame_id = "world";
    tfs_data.header.stamp = ros::Time::now();
    tfs_data.child_frame_id = "turtle";
    tfs_data.transform.translation.x = pose->x;
    tfs_data.transform.translation.y = pose->y;
    tfs_data.transform.translation.z = 0;
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, pose->theta);
    tfs_data.transform.rotation.x = qtn.getX();
    tfs_data.transform.rotation.y = qtn.getY();
    tfs_data.transform.rotation.z = qtn.getZ();
    tfs_data.transform.rotation.w = qtn.getW();
    //发布数据
    dynamic_pub.sendTransform(tfs_data);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle nh;
    //订阅乌龟位姿话题
    ros::Subscriber sub_pos = nh.subscribe("/turtle1/pose", 10, doPoseCallback);
    ros::spin();
    return 0;
}
